import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;

/**
 * A behavior that make a car drive towards light
 */
public class DriveTowardsLight extends Behavior 
{
	private SensorPort sensorLeft;
	private SensorPort sensorRight;
	private int lightThreshold = 500;
	private int MAX_LIGHT = 300;
	private int MIN_LIGHT = 600;
	
	public DriveTowardsLight( String name, int LCDrow, Behavior b)
	{
		super(name, LCDrow, b);
		
		sensorLeft = SensorPort.S2;
		sensorRight = SensorPort.S3;
		
		LightSensor l1 = new LightSensor(sensorLeft);
		LightSensor l2 = new LightSensor(sensorRight);
		//l1.setFloodlight(false);
		//l2.setFloodlight(false);
	}

	public void run()
	{    	
		while (true)
		{ 
			int leftValue, rightValue;
			do {
				leftValue = sensorLeft.readRawValue();
				rightValue = sensorRight.readRawValue();
			}
			while(leftValue > lightThreshold && rightValue > lightThreshold);
			
			suppress();

			drawString("f");
			forward(normalize(leftValue),normalize(rightValue));

			delay(10);
			
			release();
			drawString(" ");
		}
	}
	
	private int normalize(int light) {
		int output = (100 - ((light - MAX_LIGHT) * 100) / (MIN_LIGHT - MAX_LIGHT));

		if(output < 0) 
			return 0;
		if(output > 100) 
			return 100;

		return output;
	}
}
